#include "motor_ctrl.h"
/* Private includes ----------------------------------------------------------*/
#include "can_communications.h"
#include "functions.h"
#include "user_lib.h"
#include "vmc.h"
motor_data left_f;
motor_data left_b;
motor_data left_w;
motor_data right_f;
motor_data right_b;
motor_data right_w;
uint8_t set_torque_flag;
float test = 0.0f;
void Enable_All_Motor(void)
{
  static uint16_t cnt;
  static uint8_t start_flag;
  if(cnt < 2000)
  {
    cnt++;
    if(cnt == 1000)
    {
      start_flag = 1;
    }
    if(cnt == 2000)
    {
      start_flag      = 2;
      set_torque_flag = 1;
    }
  }
  if(start_flag == 0)
  {
    Set_Motor_Torque_Mode(left_f_id);

    Set_Motor_Torque_Mode(left_b_id);

    Set_Motor_Torque_Mode(left_w_id);

    Set_Motor_Torque_Mode(right_f_id);

    Set_Motor_Torque_Mode(right_b_id);

    Set_Motor_Torque_Mode(right_w_id);
  }
  if(start_flag == 1)
  {
    Set_Motor_Close_Mode(left_f_id);

    Set_Motor_Close_Mode(left_b_id);

    Set_Motor_Close_Mode(left_w_id);

    Set_Motor_Close_Mode(right_f_id);

    Set_Motor_Close_Mode(right_b_id);

    Set_Motor_Close_Mode(right_w_id);
  }
}

void Set_All_Motor_Torque(float left_f, float left_b, float left_w, float right_f, float right_b, float right_w)
{
  // left_f
  Set_Motor_Torque(left_f_id, left_f);
  // left_b
  Set_Motor_Torque(left_b_id, left_b);
  // left_w
  Set_Motor_Torque(left_w_id, left_w);
  // right_f
  Set_Motor_Torque(right_f_id, right_f);
  // right_b
  Set_Motor_Torque(right_b_id, right_b);
  // right_w
  Set_Motor_Torque(right_w_id, right_w);
}

void Motor_Control(void)
{
  Enable_All_Motor();
  if(receive_flag[0]) Motor_Rad_Cal(&left_f, -0.405614518f);
  if(receive_flag[1]) Motor_Rad_Cal(&left_b, 3.5472071717f);
  if(receive_flag[2]) Motor_Rad_Cal(&left_w, 0.0f);
  if(receive_flag[3]) Motor_Rad_Cal(&right_f, -0.405614518f);
  if(receive_flag[4]) Motor_Rad_Cal(&right_b, 3.5472071717f);
  if(receive_flag[5]) Motor_Rad_Cal(&right_w, 0.0f);
  if(set_torque_flag)
  {
    // left_f.set_torque = 0.5f;
    float_constrain(left_f.set_torque, -20.0f, 20.0f);
    float_constrain(left_b.set_torque, -20.0f, 20.0f);
    float_constrain(left_w.set_torque, -20.0f, 20.0f);
    float_constrain(right_f.set_torque, -20.0f, 20.0f);
    float_constrain(right_b.set_torque, -20.0f, 20.0f);
    float_constrain(right_w.set_torque, -20.0f, 20.0f);
    Set_All_Motor_Torque(left_f.set_torque, left_b.set_torque, left_w.set_torque, right_f.set_torque, right_b.set_torque, right_w.set_torque);
  }
}

void Motor_Rad_Cal(motor_data *data, float offset)
{
  if(!data->clear_flag)
  {
    data->last_pos   = data->pos;
    data->clear_flag = 1;
  }
  data->delta_pos  = (data->pos - data->last_pos) / 8.0f;
  data->angle     += data->delta_pos * 2 * PI;
  if(data->angle > 2 * PI)
  {
    data->angle = 0.0f;
  }
  if(data->angle < -2 * PI)
  {
    data->angle = 0.0f;
  }
  data->last_pos   = data->pos;
  data->real_angle = data->angle + offset;
}

void Set_current(motor_data *motor1, motor_data *motor2, VMC *vmc, uint8_t side)
{
  if(side == LEFT)
  {
    motor1->set_torque = -vmc->T1;
    motor2->set_torque = vmc->T2;
  }
  if(side == RIGHT)
  {
    motor1->set_torque = vmc->T1;
    motor2->set_torque = -vmc->T2;
  }
}
